#include "CameraParams.h"
#include "MvCameraControl.h"
#include "camdrv.hpp"

namespace rm_camdrv {
CamdrvNode::CamdrvNode(const rclcpp::NodeOptions &options)
    : Node("rm_camdrv", options), use_native_rgb8_(false), prefer_native_rgb8_(true) {
  RCLCPP_INFO(this->get_logger(), "Starting CamdrvNode!");

  mode_params_.emplace("default", std::make_unique<ModeParams>());
  mode_params_.emplace("Aimbot", std::make_unique<ModeParams>());
  mode_params_.emplace("Rune", std::make_unique<ModeParams>());
  mode_params_.emplace("Outpost", std::make_unique<ModeParams>());
  mode_params_.emplace("Base", std::make_unique<ModeParams>());
  mode_params_.emplace("HitFaraway", std::make_unique<ModeParams>());

  MV_CC_Initialize();

  // Get camera count
  camera_count_ = this->declare_parameter("CameraCount", 1);
  camera_handles_.resize(camera_count_);
  camera_info_msgs_.resize(camera_count_);
  camera_thread_alive_ = true;
  param_sweep_thread_alive_ = false;
  recording_thread_alive_ = false;  // Initialize recording thread status

  // enum device
  nRet = MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);

  // Check if devices satisfy the count
  while (device_list.nDeviceNum != static_cast<unsigned int>(camera_count_) && rclcpp::ok()) {
    RCLCPP_ERROR(this->get_logger(), "Found camera count = %d, expected %d!", device_list.nDeviceNum, camera_count_);
    RCLCPP_INFO(this->get_logger(), "Enum state: [%x]", nRet);
    std::this_thread::sleep_for(std::chrono::seconds(1));
    nRet = MV_CC_EnumDevices(MV_USB_DEVICE, &device_list);
  }
  RCLCPP_INFO(this->get_logger(), "Found camera count = %d", device_list.nDeviceNum);

  // Create handle
  RCLCPP_INFO(this->get_logger(), "Serial Numbers:");
  for (int i = 0; i < camera_count_; ++i) {
    MV_CC_CreateHandle(&camera_handles_[i], device_list.pDeviceInfo[i]);
    MV_CC_OpenDevice(camera_handles_[i]);
    RCLCPP_INFO(this->get_logger(), "Camera %d: %s", i, device_list.pDeviceInfo[i]->SpecialInfo.stUsb3VInfo.chSerialNumber);
  }

  bool use_sensor_data_qos =
      this->declare_parameter("use_sensor_data_qos", true);
  auto qos = use_sensor_data_qos ? rmw_qos_profile_sensor_data
                                 : rmw_qos_profile_default;
  
  // Recording parameters
  record_ = this->declare_parameter("Record", false);
  max_frames_ = this->declare_parameter("MaxFrames", 2500);
  frame_skip_interval_ = this->declare_parameter("FrameSkipInterval", 10);
  fps_ = this->declare_parameter("FPS", 30);
  record_path_ = this->declare_parameter("Path", "/home/dji/projects/record/");
  if (record_path_.back() != '/') {
    record_path_ += '/';
  }

  // Sweep parameters
  sweep_mode_ = this->declare_parameter("Sweep.On", false);
  double exposure_min = this->declare_parameter("Sweep.ExposureTime.Min", 1000.0);
  double exposure_max = this->declare_parameter("Sweep.ExposureTime.Max", 6000.1);
  double exposure_step = this->declare_parameter("Sweep.ExposureTime.Step", 500.0);
  double gain_min = this->declare_parameter("Sweep.Gain.Min", 0.8);
  double gain_max = this->declare_parameter("Sweep.Gain.Max", 16.9);
  double gain_step = this->declare_parameter("Sweep.Gain.Step", 4.0);
  sweep_interval_ = this->declare_parameter("Sweep.Interval", 500.0);

  camera_pub_ =
      image_transport::create_camera_publisher(this, "image_raw", qos);

  // Load camera info
  for (int i = 0; i < camera_count_; ++i) {
    camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(this, std::to_string(i));
    auto camera_info_url = "package://rm_camdrv/config/camera_info." + std::to_string(i) + ".yaml";
    if (camera_info_manager_->validateURL(camera_info_url)) {
      camera_info_manager_->loadCameraInfo(camera_info_url);
      camera_info_msgs_[i] = camera_info_manager_->getCameraInfo();
    } else {
      RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s",
                  camera_info_url.c_str());
    }
  }

  // Declare camera id for each mode
  for (auto &[mode, params] : mode_params_) {
    params->camera_id = this->declare_parameter<int>(mode + ".CameraID", 0);
    params->camera_handle = camera_handles_[params->camera_id];
    params->camera_info_msg = std::make_shared<sensor_msgs::msg::CameraInfo>(camera_info_msgs_[params->camera_id]);
  }

  default_camera_id_ = this->declare_parameter<int>("CameraID", 0);
  default_camera_handle_ = camera_handles_[default_camera_id_];
  current_mode_ = "default";

  // Handle mode switch
  if (!sweep_mode_) {
    readpack_sub_ = this->create_subscription<interfaces::msg::Readpack>(
        "/Readpack", 1,
        std::bind(&CamdrvNode::checkMode, this, std::placeholders::_1));
  }

  cam_info_updater_cli_ = this->create_client<rm_interfaces::srv::SetCameraID>("cam_info_updater");

  declareParameters();

  params_callback_handle_ = this->add_on_set_parameters_callback(
      std::bind(&CamdrvNode::parametersCallback, this, std::placeholders::_1));

  
  createCameraThread(default_camera_handle_);

  if (sweep_mode_) {
    exposure_time_sweep_ = SweepParams<double>(exposure_min, exposure_max, exposure_step);
    gain_sweep_ = SweepParams<double>(gain_min, gain_max, gain_step);
    // Create parameter sweep thread
    createParamSweepThread();
  }
}

CamdrvNode::~CamdrvNode() {
  RCLCPP_INFO(this->get_logger(), "CamdrvNode destructor called!");
  
  // First stop all flag bits
  camera_thread_alive_ = false;
  param_sweep_thread_alive_ = false;
  recording_thread_alive_ = false;
  
  // Wait for capture_thread_ to finish
  if (capture_thread_.joinable()) {
    RCLCPP_INFO(this->get_logger(), "Waiting for capture thread to join...");
    capture_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Capture thread joined.");
  }
  
  // Stop recording thread
  if (recording_thread_.joinable()) {
    RCLCPP_INFO(this->get_logger(), "Waiting for recording thread to join...");
    // Notify recording thread to exit
    {
      std::lock_guard<std::mutex> lock(queue_mutex_);
      queue_condition_.notify_all();
    }
    recording_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Recording thread joined.");
  }
  
  // Wait for parameter sweep thread to finish
  if (param_sweep_thread_.joinable()) {
    RCLCPP_INFO(this->get_logger(), "Waiting for param sweep thread to join...");
    param_sweep_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Param sweep thread joined.");
  }
  
  // Release recording resources
  if (video_writer_.isOpened()) {
    video_writer_.release();
    RCLCPP_INFO(this->get_logger(), "Recording stopped.");
  }
  
  // Clear recording queue
  {
    std::lock_guard<std::mutex> lock(queue_mutex_);
    while (!frame_queue_.empty()) {
      frame_queue_.pop();
    }
    RCLCPP_INFO(this->get_logger(), "Frame queue cleared.");
  }
  
  // Release camera resources
  for (auto &[mode, params] : mode_params_) {
    if (params->camera_handle) {
      RCLCPP_INFO(this->get_logger(), "Stopping camera for mode: %s", mode.c_str());
      MV_CC_StopGrabbing(params->camera_handle);
      MV_CC_CloseDevice(params->camera_handle);
      MV_CC_DestroyHandle(&params->camera_handle);
      params->camera_handle = nullptr;
    }
  }
  
  // Finally cleanup camera driver
  MV_CC_Finalize();
  RCLCPP_INFO(this->get_logger(), "CamdrvNode destroyed!");
}
} // namespace rm_camdrv

#include "rclcpp_components/register_node_macro.hpp"

RCLCPP_COMPONENTS_REGISTER_NODE(rm_camdrv::CamdrvNode)
